#ifndef MODEL_INVERSE_DYNAMICS_H
#define MODEL_INVERSE_DYNAMICS_H

#include "basic_data_structure.hpp"
#include "forward_kinematics.hpp"

namespace ZORAL
{
    class InverseDynamics
    {
    private:
        std::shared_ptr<ForwardKinematics> fk_;
    public:
        explicit InverseDynamics(std::shared_ptr<ForwardKinematics> fk)
        {
            fk_ = std::move(fk);
        }

        int inverseDynamics(const VectorXd &q, const VectorXd &qd, const VectorXd &qdd,
                            const std::vector<Wrench> &f_ext, VectorXd &torque) const;

        int inverseDynamics(const VectorXd &q, const VectorXd &qd, const VectorXd &qdd, VectorXd &torque) const;
    };
} // namespace ZORAL

#endif